#include <iostream>
#include <Eigen/Dense>
#include <math.h>

#include "KinematicModelTypes.h"
#include "ArmDimensions.h"	

dirKinResult dirKin(float teta1, float teta2, float teta3);

dirKinResult dirKin(float teta1, float teta2, float teta3, float teta4){


	//tetai: angolo fra l'asse Xi-1 e Xi attorno a Zi-1 (variabile nel giunto
	//rotoidale)
	//alfai: angolo fra l'asse Zi-1 e Zi intorno a Xi (angolo di twist)
	//ai: distanza tra Zi-1 e Zi lungo l'asse Xi (lunghezza del link)
	//di: distanza tra Xi-1 e Xi lungo l'asse Zi-1 (Variabile nel giunto prismatico)

	dirKinResult res;

	float teta0 = M_PI/2.0;								//Rotazione del supporto rispetto all'asse Z
	float alfa0 = M_PI/2.0;								//Rotazione del supporto rispetto all'asse Y
	float a0 = SHOULDERY;										//Coordinata Y del supporto
	float d0 = SHOULDERZ;										//Coordinata Z del supporto

	float teta1s = (3.0/2.0)*M_PI+(teta1*M_PI/180.0);
	float alfa1 = M_PI/2.0;
	float a1 = 0;
	float d1 = SHOULDERX;										//Lunghezza del supporto (coordinata X della spalla)

	float teta2s = (teta2*M_PI/180.0);
	float alfa2 = (3.0/2.0)*M_PI;
	float a2 = FIRST_LINK_LENGTH;										//Lunghezza del primo link
	float d2 = 0;        

	float teta3s = M_PI/2.0+(teta3*M_PI/180.0);
	float alfa3 = M_PI/2.0;
	float a3 = 0;
	float d3 = 0;

	float teta4s = teta4*M_PI/180.0;
	float alfa4 = 0;
	float a4 = 0;
	float d4 = SECOND_LINK_LENGTH;										//Lunghezza del secondo link


	
	Eigen::Matrix4f Aw;
	Eigen::Matrix4f A01, A02, A03, A04;
	Eigen::Matrix4f A12;
	Eigen::Matrix4f A23;
	Eigen::Matrix4f A34;
	
	Aw << cos(teta0), (-cos(alfa0)*sin(teta0)), (sin(alfa0)*sin(teta0)), (a0*cos(teta0)),
		sin(teta0), (cos(alfa0)*cos(teta0)), (-sin(alfa0)*cos(teta0)), (a0*sin(teta0)),
		0, sin(alfa0), cos(alfa0), d0,
		0, 0, 0, 1;

	A01 << cos(teta1s), (-cos(alfa1)*sin(teta1s)), (sin(alfa1)*sin(teta1s)), (a1*cos(teta1s)),
		sin(teta1s), (cos(alfa1)*cos(teta1s)), (-sin(alfa1)*cos(teta1s)), (a1*sin(teta1s)),
		0, sin(alfa1), cos(alfa1), d1,
		0, 0, 0, 1;

	A12 << cos(teta2s), (-cos(alfa2)*sin(teta2s)), (sin(alfa2)*sin(teta2s)), (a2*cos(teta2s)),
		sin(teta2s), (cos(alfa2)*cos(teta2s)), (-sin(alfa2)*cos(teta2s)), (a2*sin(teta2s)),
		0, sin(alfa2), cos(alfa2), d2,
		0, 0, 0, 1;

	A23 << cos(teta3s), (-cos(alfa3)*sin(teta3s)), (sin(alfa3)*sin(teta3s)), (a3*cos(teta3s)),
		sin(teta3s), (cos(alfa3)*cos(teta3s)), (-sin(alfa3)*cos(teta3s)), (a3*sin(teta3s)),
		0, sin(alfa3), cos(alfa3), d3,
		0, 0, 0, 1;

	A34 << cos(teta4s), -cos(alfa4)*sin(teta4s), sin(alfa4)*sin(teta4s), a4*cos(teta4s),
		sin(teta4s), cos(alfa4)*cos(teta4s), -sin(alfa4)*cos(teta4s), a4*sin(teta4s),
		0, sin(alfa4), cos(alfa4), d4,
		0, 0, 0, 1;

	A01 = Aw * A01;
	A02 = A01 * A12;
	A03 = A02 * A23;
	A04 = A03 * A34;
	
	Eigen::MatrixXf PW(4,1);
	Eigen::MatrixXf XW(4,1);
	Eigen::MatrixXf YW(4,1);
	Eigen::MatrixXf ZW(4,1);

	PW << 0, 0, 0, 1;
	XW << 1, 0, 0, 1;
	YW << 0, 1, 0, 1;
	ZW << 0, 0, -1, 1;

	Eigen::MatrixXf P0(4,1);
	Eigen::MatrixXf X0(4,1);
	Eigen::MatrixXf Y0(4,1);
	Eigen::MatrixXf Z0(4,1);

	P0 = Aw*PW;
	X0 = Aw*XW;
	Y0 = Aw*YW;
	Z0 = Aw*ZW;

	Eigen::MatrixXf P1(4,1);
	Eigen::MatrixXf X1(4,1);
	Eigen::MatrixXf Y1(4,1);
	Eigen::MatrixXf Z1(4,1);

	P1 = A01*PW;
	X1 = A01*XW;
	Y1 = A01*YW;
	Z1 = A01*ZW;
	
	Eigen::MatrixXf P2(4,1);
	Eigen::MatrixXf X2(4,1);
	Eigen::MatrixXf Y2(4,1);
	Eigen::MatrixXf Z2(4,1);

	P2 = A02*PW;
	X2 = A02*XW;
	Y2 = A02*YW;
	Z2 = A02*ZW;

	Eigen::MatrixXf P3(4,1);
	Eigen::MatrixXf X3(4,1);
	Eigen::MatrixXf Y3(4,1);
	Eigen::MatrixXf Z3(4,1);

	P3 = A03*PW;//
	X3 = A03*XW;
	Y3 = A03*YW;
	Z3 = A03*ZW;
	
	Eigen::MatrixXf P4(4,1);
	Eigen::MatrixXf X4(4,1);
	Eigen::MatrixXf Y4(4,1);
	Eigen::MatrixXf Z4(4,1);

	P4 = A04*PW;//
	X4 = A04*XW;
	Y4 = A04*YW;
	Z4 = A04*ZW;

	Eigen::MatrixXf aux1(4,1), aux2(4,1), aux3(4,1), aux4(4,1), auxc(4,1);
	auxc << 0, 0, HAND_CENTER_OFFSET, 1;
	
	aux1 << 0, -HAND_CENTER_OFFSET, 0.08, 1;
	aux2 << 0, -HAND_CENTER_OFFSET, 0, 1;
	aux3 << 0, HAND_CENTER_OFFSET, 0, 1;
	aux4 << 0, HAND_CENTER_OFFSET, 0.08, 1;


	Eigen::Vector4f mc = A04*auxc;	//Posizione della mano
	
	Eigen::Vector4f m1 = A04*aux1;	//Posizione della mano
	Eigen::Vector4f m2 = A04*aux2;	//Posizione della mano
	Eigen::Vector4f m3 = A04*aux3;	//Posizione della mano
	Eigen::Vector4f m4 = A04*aux4;	//Posizione della mano
	
	/*//Truncating float digits
	char sz[64];
	sprintf(sz, "%.4lf\n", P1(0)); 
	P1(0) = atof(sz);
	sprintf(sz, "%.4lf\n", P1(1)); 
	P1(1) = atof(sz);
	sprintf(sz, "%.4lf\n", P1(2)); 
	P1(2) = atof(sz);
	
	sprintf(sz, "%.4lf\n", P3(0)); 
	P3(0) = atof(sz);
	sprintf(sz, "%.4lf\n", P3(1)); 
	P3(1) = atof(sz);
	sprintf(sz, "%.4lf\n", P3(2)); 
	P3(2) = atof(sz);

	sprintf(sz, "%.4lf\n", P4(0)); 
	P4(0) = atof(sz);
	sprintf(sz, "%.4lf\n", P4(1)); 
	P4(1) = atof(sz);
	sprintf(sz, "%.4lf\n", P4(2)); 
	P4(2) = atof(sz);
	
	sprintf(sz, "%.4lf\n", mc(0)); 
	mc(0) = atof(sz);
	sprintf(sz, "%.4lf\n", mc(1)); 
	mc(1) = atof(sz);
	sprintf(sz, "%.4lf\n", mc(2)); 
	mc(2) = atof(sz);
	
	sprintf(sz, "%.4lf\n", m1(0)); 
	m1(0) = atof(sz);
	sprintf(sz, "%.4lf\n", m1(1)); 
	m1(1) = atof(sz);
	sprintf(sz, "%.4lf\n", m1(2)); 
	m1(2) = atof(sz);
	
	
	sprintf(sz, "%.4lf\n", m2(0)); 
	m2(0) = atof(sz);
	sprintf(sz, "%.4lf\n", m2(1)); 
	m2(1) = atof(sz);
	sprintf(sz, "%.4lf\n", m2(2)); 
	m2(2) = atof(sz);
	
	
	sprintf(sz, "%.4lf\n", m3(0)); 
	m3(0) = atof(sz);
	sprintf(sz, "%.4lf\n", m3(1)); 
	m3(1) = atof(sz);
	sprintf(sz, "%.4lf\n", m3(2)); 
	m3(2) = atof(sz);
	
	
	sprintf(sz, "%.4lf\n", m4(0)); 
	m4(0) = atof(sz);
	sprintf(sz, "%.4lf\n", m4(1)); 
	m4(1) = atof(sz);
	sprintf(sz, "%.4lf\n", m4(2)); 
	m4(2) = atof(sz);*/
	
	
	
	/*std::cout << "Posizione della spalla: \n" << P1 << "\n\n" << std::endl;
	std::cout << "Posizione del gomito: \n" << P3 << "\n\n" << std::endl;
	std::cout << "Posizione del polso: \n" << P4 << "\n\n" << std::endl;
	std::cout << "Posizione della mano: \n" << mc << std::endl;*/
	

	res.shoulder = new Point(P1(0), P1(1), P1(2));
	
	res.elbow = new Point(P3(0), P3(1), P3(2));
	
	res.wrist = new Point(P4(0), P4(1), P4(2));
	
	res.handCenter = new Point(mc(0), mc(1), mc(2));
	
	res.graspingPoint1 = new Point(m1(0), m1(1), m1(2));
	
	res.graspingPoint2 = new Point(m4(0), m4(1), m4(2));
	
	res.fingerPoint1 = new Point((m1(0) + m2(0))/2, (m1(1) + m2(1))/2, (m1(2) + m2(2))/2);

	res.fingerPoint2 = new Point((m3(0) + m4(0))/2, (m3(1) + m4(1))/2, (m3(2) + m4(2))/2);


	/*res.fingerPoint1.x = (m1(0) + m2(0))/2;
	res.fingerPoint1.y = (m1(1) + m2(1))/2;
	res.fingerPoint1.z = (m1(2) + m2(2))/2;

	res.fingerPoint2.x = (m3(0) + m4(0))/2;
	res.fingerPoint2.y = (m3(1) + m4(1))/2;
	res.fingerPoint2.z = (m3(2) + m4(2))/2;*/
	

	return res;
	
}


dirKinResult dirKin(float teta1, float teta2, float teta3){

	return dirKin(teta1, teta2, teta3, 0);

}